import app as robot_script

class Mode:

    def __init__(self, framework):
        self.framework = framework
        self.succeed_state = False

    # 设置模式
    def set_mode(self, mode):
        if mode == "stand_up":
            self.on_stand_up()
        if mode == "get_down":
            self.on_get_down()
        if mode == "on_pattern_gait_low_trot":
            self.on_pattern_gait_low_trot()
        if mode == "on_pattern_gait_trot":
            self.on_pattern_gait_trot()
        if mode == "on_pattern_gait_stand":
            self.on_pattern_gait_stand()

    # 站立动作
    def on_stand_up(self):
        if self.framework.cyberdog_subscribe is not None:
            request = self.framework.cyberdog_pb2.CheckoutMode_request()
            request.next_mode.mode.control_mode = self.framework.cyberdog_pb2.CheckoutMode_request.MANUAL
            request.timeout = 0
            response = self.framework.cyberdog_subscribe.setMode(request)
            for resp in response:
                self.succeed_state = resp.succeed
                self.framework.framework.common.socket.log("Mode on_stand_up, result:" + str(self.succeed_state))

    # 趴下动作
    def on_get_down(self):
        if self.framework.cyberdog_subscribe is not None:
            request = self.framework.cyberdog_pb2.CheckoutMode_request()
            request.next_mode.mode.control_mode = self.framework.cyberdog_pb2.CheckoutMode_request.DEFAULT
            request.timeout = 0
            response = self.framework.cyberdog_subscribe.setMode(request)
            for resp in response:
                self.succeed_state = resp.succeed
                self.framework.framework.common.socket.log("Mode on_get_down, result:" + str(self.succeed_state))

    # 恢复站立
    def on_pattern_gait_stand(self):
        if self.framework.cyberdog_subscribe is not None:
            request = self.framework.cyberdog_pb2.CheckoutPattern_request()
            request.patternstamped.pattern.gait_pattern = self.framework.cyberdog_pb2.Pattern.GAIT_STAND_R
            request.timeout = 0
            response = self.framework.cyberdog_subscribe.setPattern(request)
            for resp in response:
                self.succeed_state = resp.succeed
                self.framework.framework.common.socket.log("Mode on_pattern_gait_stand, result:" + str(self.succeed_state))

    # 小跑步态
    def on_pattern_gait_low_trot(self):
        if self.framework.cyberdog_subscribe is not None:
            request = self.framework.cyberdog_pb2.CheckoutPattern_request()
            request.patternstamped.pattern.gait_pattern = self.framework.cyberdog_pb2.Pattern.GAIT_SLOW_TROT
            request.timeout = 0
            response = self.framework.cyberdog_subscribe.setPattern(request)
            for resp in response:
                self.succeed_state = resp.succeed
                self.framework.framework.common.socket.log("Mode on_pattern_gait_low_trot, result:" + str(self.succeed_state))

    # 小跑步态
    def on_pattern_gait_trot(self):
        if self.framework.cyberdog_subscribe is not None:
            request = self.framework.cyberdog_pb2.CheckoutPattern_request()
            request.patternstamped.pattern.gait_pattern = self.framework.cyberdog_pb2.Pattern.GAIT_TROT
            request.timeout = 0
            response = self.framework.cyberdog_subscribe.setPattern(request)
            for resp in response:
                self.succeed_state = resp.succeed
                self.framework.framework.common.socket.log("Mode on_pattern_gait_trot, result:" + str(self.succeed_state))
